#include "arm_control.h"
#include <thread>


ArmControl ac;


void client(){
    const unsigned short SERVERPORT = 5000;
    const char* SERVER_IP = "192.168.1.100";
    int sock=ac.connect_to_robot(SERVER_IP,SERVERPORT);
    if(sock==-1)return;

    float l_angle[6]={0};
    l_angle[0]=160.207;
    l_angle[1]=-16.945;
    l_angle[2]=-74.708;
    l_angle[3]=14.1951;
    l_angle[4]=-82.859;
    l_angle[5]=10.546;

    float r_angle[6]={0};
    r_angle[0]=159.207;
    r_angle[1]=-16.945;
    r_angle[2]=-74.708;
    r_angle[3]=14.1951;
    r_angle[4]=-82.859;
    r_angle[5]=10.546;

    float l_pose[6]={0};
    l_pose[0]=20;
    l_pose[1]=0;
    l_pose[2]=0;
    l_pose[3]=0;
    l_pose[4]=0;
    l_pose[5]=0;
    float r_pose[6]={0};
    r_pose[0]=20;
    r_pose[1]=0;
    r_pose[2]=0;
    r_pose[3]=0;
    r_pose[4]=0;
    r_pose[5]=0;

    //ac.arm_init(); // 初始化
//    while(1) { // 速度和角速度必须循环发，其他只发一次
        ac.client_get_data();
////        ac.print_status_now();
//        sleep(0.2);
//        ac.set_wheel(0, -0.01); // 角速度 轮子有时候角速度和速度都不好使
////        ac.set_wheel(3, 0); // 速度
//    }
    //ac.set_up_down(0); // 举升
        //ac.arm_init();
        //ac.clear_warning(); //清报警 之后需要初始化 ac.arm_init();


        //ac.set_up_down(50);
        //ac.set_wheel(0,-0.01);
        //ac.arm_init();

        //ac.set_l_angle(l_angle);
        //ac.set_r_angle(r_angle);
        //ac.set_lr_angle(l_angle,r_angle);
        //ac.set_up_down(20);
        //ac.set_l_pose(r_pose);
        //ac.set_l_pose(r_pose);
        //r_pose[0]=-30;
        //ac.set_lr_angle(l_angle,r_angle);
        //ac.set_r_pose(r_pose);
        //ac.set_l_pose(r_pose); // 左臂发位置不动

        //ac.set_lr_pose(r_pose,r_pose);
        //ac.set_zero();
        //ac.set_wheel(0,-0.01);
//        while(1){
//            ac.client_get_data();
//            ac.print_status_now();
//            sleep(1);
//            //ac.arm_init();
//            ac.clear_warning();
//            //ac.set_up_down(10);
//        }
    //}
}

int main(){

    //thread t2(client);
    //t2.join();

    client();


    return 0;
}

